#include "pcl_process.hpp"

//定位系统数据
float OpsMsg_x = 0;
float OpsMsg_y = 0;
float OpsMsg_yaw = 0;
float table_yaw = 0;

/**
 * @brief 点云显示初始化
 * @param 
 * @return 
 * @details 
*/
void PclProcess::Pcl_show_init()
{
    // 设置背景颜色
    viewer.setBackgroundColor(255, 255, 255);
    // 设置坐标轴
    viewer.addCoordinateSystem(0.2);
    // 设置 视角 变换
    viewer.setCameraPosition(0, -0.449*15, -0.443, 0, 0, 0, 0, 0, 0, 1);
}

/**
 * @brief 点云显示
 * @param 需要显示的点云
 * @return 
 * @details 
*/
void PclProcess::Pcl_show(void)
{
    // 清空之前的数据
    viewer.removeAllPointClouds();
    
    // 设置按x，y，z方向显色
    // pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> fildColor(cloud_points, "z");
    // pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> fildColor(phase_cloud, "z");
    // 设置单色显示
    // pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> singleColor(roi_cloud, 0,0,0);
    // pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> singleColor2(center_cloud, 255,0,0);
    // pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> singleColor3(phase_cloud, 0,0,0);
    // 添加雷达数据点云
    viewer.addPointCloud(roi_cloud, "roi_cloud");
    // viewer.addPointCloud(center_cloud, "center_cloud");
    // viewer.addPointCloud(cloud_sphere, "cloud_sphere");
    // viewer.addPointCloud(phase_cloud, singleColor3, "phase_cloud");
    // viewer.addPointCloud(cloud_points);
    // 更改点云点大小和颜色
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "roi_cloud");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 0, 1, "roi_cloud");
    // viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "center_cloud");
    // viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, "center_cloud");
    // viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud_sphere");
    // viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 0, 1, "cloud_sphere");

    // 更新可视化器 设置适当的延迟以实现实时更新
    viewer.spinOnce(1);
}

/**
 * @brief 文件夹内点云显示
 * @param 需要显示的点云编号
 * @return 
 * @details 
*/
void PclProcess::Pcl_show(int& num)
{
    // 清空之前的数据
    viewer.removeAllPointClouds();
    if(num < 0)num = cloud_allpath.size() - 1;
    if(num >= cloud_allpath.size())num = 0;
    

    pPointCloud disp_cloud = loadPointCloud(cloud_allpath.at(num));
    cout << cloud_allpath.at(num) << endl;
    viewer.addPointCloud(disp_cloud, "disp_cloud");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "disp_cloud");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 0, 1, "disp_cloud");

    // 更新可视化器 设置适当的延迟以实现实时更新
    viewer.spinOnce(1);
}

/**
 * @brief 点云保存
 * @param 需要保存的点云和路径
 * @return 
 * @details 保存格式为.ply
*/
void PclProcess::savePointCloud(const pPointCloud cloud, const string outpath)
{
    // char *path = new char[outpath.size() +1];
    // strcpy(path , outpath.c_str());
	
    //写出点云图
    pcl::PLYWriter writer;
    writer.write(outpath, *cloud, true, false);

}

/**
 * @brief 点云读取
 * @param 需要读取的路径
 * @return 点云指针
 * @details 
*/
pPointCloud PclProcess::loadPointCloud(const string path)
{

    pPointCloud cloud(new PointCloud);
    if (pcl::io::loadPLYFile<pcl::PointXYZ>(path, *cloud) == -1) 
    {       
        PCL_ERROR("Couldnot read file.\n");
        return 0;
    }

    return cloud;
}

/**
 * @brief 获取文件路径列表
 * @param 需要读取的文件夹路径
 * @return 包含所有路径的容器
 * @details 
*/
vector<string> PclProcess::getpath(string dirpath)
{
    DIR *dir = opendir(dirpath.c_str());
    if (dir == NULL)
    {
        cout << "opendir error" << endl;
    }
    vector<string> allPath;

    struct dirent **namelist;
    int n = scandir(dirpath.c_str(), &namelist, NULL, versionsort);
    if(n < 0)
        cerr << "memery error " << endl;
    else
        cout << "total number is: " << n << endl;
    for( int i = 2; i < n; i++)
    {
        string str3;
        string str1=dirpath+"/";
        string str2=namelist[i]->d_name;
        str3 = str1 + str2;
        allPath.push_back(str3);
        // cout<<str3<<endl;

    }
    free(namelist);
    closedir(dir);
    return allPath;
}

/**
 * @brief 中央区域均值法获取球心位置
 * @param 包含球的点云，识别出的球的矩形框位置
 * @return 一个点坐标
 * @details 此处的点云应为识别出球后根据矩形框分割出的点云
*/
PointType PclProcess::get_ball_center(pPointCloud cloud_points, cv::Rect roiRect)
{
    int start_i = int(int(0.25 * roiRect.height) * roiRect.width + 0.25 * roiRect.width);
    int end_i = int(int(0.75 * roiRect.height) * roiRect.width + 0.25 * roiRect.width);
    int gap_val_width = int(0.5 * roiRect.width / CLOUD_DENSITY_THRESHOULD + 1);
    int gap_val_height = int(0.5 * roiRect.height / CLOUD_DENSITY_THRESHOULD + 1);
    float sum_x = 0;
    float sum_y = 0;
    float sum_z = 0;
    int efficient_num = 0;
    float point_scale = 0;
    float point_length = 0;

    PointType temp_point(0,0,0);
    PointType center_point_fun(0,0,0);
    PointType output_point(0,0,0);
    pcl::PointCloud<pcl::PointXYZ>::iterator it = cloud_points->begin();
    
    // 平移旋转点云
    cloud_translation(cloud_points, 0, -0.52, -0.3915, 22, 0, 0);

    // 裁剪出中间长宽为输入长宽0.5倍的点云
    for(int i = start_i; i <= end_i; i = i + roiRect.width * gap_val_height)
    {
        for(int j = i; j <= i + 0.5 * roiRect.width; j = j + gap_val_width)
        {
            it = cloud_points->begin() + j;
            temp_point.x = it->x;
            temp_point.y = it->y;
            temp_point.z = it->z;

            // cout << "temp_ponit "<< i << "	" << j << "	" << temp_point << endl;

            center_cloud->points.push_back(temp_point);
        }
    }  
    cout << "center_cloud_size: " << "	" << center_cloud->size() << endl;
    cout << "start_i: " << "	" << start_i << "   " << end_i << endl;

    // std::sort(center_cloud->begin(), center_cloud->end(), [](pcl::PointXYZ pt1, pcl::PointXYZ pt2) {return pt1.z < pt2.z; });
    // 对坐标值求和，求平均
    for (it = center_cloud->begin(); it != center_cloud->end(); ++it)
    {
        // cout << "center_cloud: " << "	" << *it << endl;
        // temp_point.x = it->x;
        // temp_point.y = it->y;
        // temp_point.z = it->z;
        // center_cloud_filtered->points.push_back(temp_point);
        if(it->x != 0 && it->y != 0 && it->z != 0)
        {
            sum_x = sum_x + it->x;
            sum_y = sum_y + it->y;
            sum_z = sum_z + it->z;
            efficient_num++;
        }
    }  
    center_point_fun.x = sum_x / efficient_num;
    center_point_fun.y = sum_y / efficient_num;
    center_point_fun.z = sum_z / efficient_num;
    // 去掉与z轴平均值相差过远的点
    sum_x = 0;
    sum_y = 0;
    sum_z = 0;
    efficient_num = 0;
    for (it = center_cloud->begin(); it != center_cloud->end(); ++it)
    {
        if(it->z - center_point_fun.z < BALLRADIUS || it->z - center_point_fun.z > -BALLRADIUS)
        {
            sum_x = sum_x + it->x;
            sum_y = sum_y + it->y;
            sum_z = sum_z + it->z;
            efficient_num++;
        }
    }
    center_point_fun.x = sum_x / efficient_num;
    center_point_fun.y = sum_y / efficient_num;
    center_point_fun.z = sum_z / efficient_num;

    // 沿计算出的中心点向后延伸一个半径，得到球心
    point_length = pow(pow(center_point_fun.x, 2)+pow(center_point_fun.y, 2)+pow(center_point_fun.z, 2), 0.5);
    point_scale = (point_length + BALLRADIUS) / point_length;
    output_point.x = center_point_fun.x * point_scale;
    output_point.y = center_point_fun.y * point_scale;
    output_point.z = center_point_fun.z * point_scale;
    cout << "output_point: " << "	" << output_point << endl;

    // 点云保存
    #ifdef CLOUD_SAVE
    gettimeofday(&time_pcl, NULL);
    savePointCloud(cloud_points, format("./data/frame/%d_%d.ply", time_pcl.tv_sec, time_pcl.tv_usec));
    #endif

    return output_point;
}

/**
 * @brief 中央区域均值法获取球心位置
 * @param 包含球的点云，识别出的球的矩形框位置
 * @return 一个点坐标
 * @details 此处的点云应为识别出球后根据矩形框分割出的点云
*/
PointType PclProcess::get_ball_center2(pPointCloud cloud_points, PointType reference_point)
{
    PointType start_point(-3,-5,0);
    PointType end_point(3,4,7);
    PointType output_point(0,0,0);
    float min_z = 0;

    // 裁剪点云并下采样
    pcl::VoxelGrid<pcl::PointXYZ> vg;
    pPointCloud cloud_filtered(new PointCloud);
    vg.setInputCloud (cloud_clip_3d(cloud_points, start_point, end_point));
    vg.setLeafSize (0.01f, 0.01f, 0.01f);
    vg.filter (*cloud_filtered);
    cout << "cloud_filtered_size: " << "	" << cloud_filtered->size() << endl;

    // // 平移旋转点云
    // cloud_translation(cloud_filtered, 0, -0.413, -0.390, 32, 0, 0);

    // 创建用于提取搜索方法的kdtree树对象
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
    tree->setInputCloud (cloud_filtered);
    // 欧式聚类
    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;   //欧式聚类对象
    ec.setClusterTolerance (0.05);                     // 设置近邻搜索的搜索半径
    ec.setMinClusterSize (5);                 //设置一个聚类需要的最少的点数目
    ec.setMaxClusterSize (1000);               //设置一个聚类需要的最大点数目
    ec.setSearchMethod (tree);                    //设置点云的搜索机制
    ec.setInputCloud (cloud_filtered);
    ec.extract (cluster_indices);           //从点云中提取聚类，并将点云索引保存在cluster_indices中
    cout << "cluster_indices_size: " << "	" << cluster_indices.size() << endl;
    // 找出聚类中距离摄像头最近的，作为球点云
    if(cluster_indices.size() > 0)
    {
        int it_num = 0;
        min_z = 999;
        int min_num = 0;
        for(std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end(); ++it)
        {
            float sum_z = 0;
            float mean_z = 0;
            for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
            {
                sum_z = sum_z + cloud_filtered->points[*pit].z;
            }
            mean_z = sum_z / it->indices.size();
            if(mean_z < min_z)
            {
                min_z = mean_z;
                min_num = it_num;
            }

            it_num ++;
        }
        std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin () + min_num;
        for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
        {
            center_cloud->points.push_back (cloud_filtered->points[*pit]);
        }
    }
    cout << "center_cloud_size: " << "	" << center_cloud->size() << endl;
    // 根据深度算出簇中心点坐标（非球心）
    if(min_z != 0){
        output_point.x = reference_point.x * min_z; // 取中心点时默认深度为1，所以这里省去了除1
        output_point.y = reference_point.y * min_z;
        output_point.z = min_z;
    }
    // 平移旋转点
    output_point = cloud_translation(output_point, 0, -0.412426, -0.386726, 20, 0, 0);
    cout << "center_point: " << "	" << output_point << endl;
    
    // 点云保存
    #ifdef CLOUD_SAVE
    gettimeofday(&time_pcl, NULL);
    savePointCloud(center_cloud, format("./data/frame/%d_%d.ply", time_pcl.tv_sec, time_pcl.tv_usec));
    gettimeofday(&time_pcl, NULL);
    savePointCloud(cloud_points, format("./data/frame/%d_%d.ply", time_pcl.tv_sec, time_pcl.tv_usec));
    #endif

    return output_point;
}

/**
 * @brief RANSAC拟合球面获取球心位置
 * @param 包含球的点云，识别出的球的矩形框位置
 * @return 一个点坐标
 * @details 此处的点云应为识别出球后根据矩形框分割出的点云
*/
PointType PclProcess::get_ball_center_ransac(pPointCloud cloud_points, cv::Rect roiRect)
{
    
    PointType temp_point(0,0,0);
    PointType output_point(0,0,0);
    PointType start_point(-1,-2,0);
    PointType end_point(1,2,2);

    // 裁剪点云并下采样
    pcl::VoxelGrid<pcl::PointXYZ> vg;
    pPointCloud cloud_filtered(new PointCloud);
    vg.setInputCloud (cloud_clip_3d(cloud_points, start_point, end_point));
    vg.setLeafSize (0.01f, 0.01f, 0.01f);
    vg.filter (*cloud_filtered);
    cout << "cloud_filtered_size: " << "	" << cloud_filtered->size() << endl;

    // 平移旋转点云
    // cloud_translation(cloud_filtered, 0, -0.349554, -0.354845, 20, 0, 90);

    // 创建用于提取搜索方法的kdtree树对象
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
    tree->setInputCloud (cloud_filtered);
    // 欧式聚类
    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;   //欧式聚类对象
    ec.setClusterTolerance (0.03);                     // 设置近邻搜索的搜索半径
    ec.setMinClusterSize (5);                 //设置一个聚类需要的最少的点数目
    ec.setMaxClusterSize (2000);               //设置一个聚类需要的最大点数目
    ec.setSearchMethod (tree);                    //设置点云的搜索机制
    ec.setInputCloud (cloud_filtered);
    ec.extract (cluster_indices);           //从点云中提取聚类，并将点云索引保存在cluster_indices中
    cout << "cluster_indices_size: " << "	" << cluster_indices.size() << endl;
    // 找出聚类中距离摄像头最近的，作为球点云,同时裁切点云，按距离阈值去除拖尾
    if(cluster_indices.size() > 0)
    {
        int it_num = 0;
        float min_z = 999;
        int min_num = 0;
        vector<float> front_z;
        for(std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end(); ++it)
        {
            float sum_z = 0;
            float mean_z = 0;
            float front_z_min = 999;
            for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
            {
                sum_z = sum_z + cloud_filtered->points[*pit].z;
                if(cloud_filtered->points[*pit].z < front_z_min) front_z_min = cloud_filtered->points[*pit].z;
            }
            mean_z = sum_z / it->indices.size();
            front_z.push_back(front_z_min);
            if(mean_z < min_z)
            {
                min_z = mean_z;
                min_num = it_num;
            }
            it_num ++;
            cout << it_num << "	" << it->indices.size() << "    " << mean_z << endl;
        }
        std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin () + min_num;
        float front_dis_thres = front_z[min_num] + 0.05;
        for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
        {
            if(cloud_filtered->points[*pit].z < front_dis_thres) center_cloud->points.push_back (cloud_filtered->points[*pit]);
        }

    }
    cout << "center_cloud_size: " << "	" << center_cloud->size() << endl;
    
    // 拟合球
    if(center_cloud->size() < 10){return output_point;}
    // pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr model_sphere(new pcl::SampleConsensusModelSphere<pcl::PointXYZ>(center_cloud));	
	// pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_sphere);
	// ransac.setDistanceThreshold(0.01);	//设置距离阈值，与球面距离小于0.1的点作为内点
	// ransac.computeModel();				//执行模型估计
    // // 提取内点/
	// // pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_sphere(new pcl::PointCloud<pcl::PointXYZ>);
	// vector<int> inliers;				//存储内点索引的容器
	// ransac.getInliers(inliers);			//提取内点索引
	// pcl::copyPointCloud<pcl::PointXYZ>(*center_cloud, inliers, *cloud_sphere);
	// // 输出球模型参数
	// Eigen::VectorXf coefficient;
	// ransac.getModelCoefficients(coefficient);
    // cout << "球面方程为：\n" << "(x - " << coefficient[0] << ") ^ 2 + (y - " << coefficient[1]
	// 	<< ") ^ 2 + (z - " << coefficient[2] << ")^2 = " << coefficient[3] << " ^2" << endl;
    // // 根据半径筛选符合条件的球
    // if(coefficient[3] > BALLRADIUS_MIN && coefficient[3] < BALLRADIUS_MAX)
    // {
    //     output_point.x = coefficient[0];
    //     output_point.y = coefficient[1];
    //     output_point.z = coefficient[2];
    // }
    
    VectorXf result = FitterLeastSquareMethod(center_cloud);
    if(result[3] > BALLRADIUS_MIN && result[3] < BALLRADIUS_MAX){
        output_point.x = result[0];
        output_point.y = result[1];
        output_point.z = result[2];
    }

    // 将球心坐标从摄像头坐标系转换到世界坐标系
    output_point = Ball_Relative_To_World_Coordinate(0.0, 0.0, 0.0, 0.0, output_point);
	cout << "center_point: " << "	" << output_point << "  radius:     " << result[3] << endl;

    // 点云保存
    #ifdef CLOUD_SAVE
    gettimeofday(&time_pcl, NULL);
    savePointCloud(center_cloud, format("./data/frame/%d_%d.ply", time_pcl.tv_sec, time_pcl.tv_usec));
    gettimeofday(&time_pcl, NULL);
    savePointCloud(cloud_points, format("./data/frame/%d_%d.ply", time_pcl.tv_sec, time_pcl.tv_usec));
    #endif

    return output_point;
}

/**
 * @brief 最小二乘法拟合球面
 * @param X X轴数据 
 * @param Y Y轴数据
 * @param Z Z轴数据
 * @return Eigen::VectorXf 拟合球面的系数向量
 */
VectorXf PclProcess::FitterLeastSquareMethod(const pPointCloud pointCloud) {
    vector<float> X;
    vector<float> Y;
    vector<float> Z;
    for(size_t i = 0; i < pointCloud->size(); i++){
        X.push_back(pointCloud->at(i).x);
        Y.push_back(pointCloud->at(i).y);
        Z.push_back(pointCloud->at(i).z);
    }

    MatrixXf mtxVandermonde(X.size(), 4);
    VectorXf rowVandermonde(4);
    VectorXf colVandermonde(X.size());

    for (size_t i = 0; i < X.size(); i++) {
        // 系数向量
        rowVandermonde[0] = (-2) * X[i];
        rowVandermonde[1] = (-2) * Y[i];
        rowVandermonde[2] = (-2) * Z[i];
        rowVandermonde[3] = 1;
        // 系数矩阵
        mtxVandermonde.row(i) = rowVandermonde.transpose();
        // 等号右侧值向量
        colVandermonde[i] = -(X[i] * X[i] + Y[i] * Y[i] + Z[i] * Z[i]);
    }

    VectorXf result = (mtxVandermonde.transpose()*mtxVandermonde).inverse()*(mtxVandermonde.transpose())*colVandermonde;
    result[3] = pow((result[0] * result[0] + result[1] * result[1] + result[2] * result[2] - result[3]), 0.5);
    return result;
}

/**
 * @brief 点云裁剪
 * @param 输入点云，起始坐标，中点坐标
 * @return 裁剪后的点云
 * @details 保留起始点和终点构成的四面体内的点
*/
pPointCloud PclProcess::cloud_clip_3d(pPointCloud cloud_points, PointType start_point, PointType end_point)
{
    float start_x = 0, end_x = 0;
    float start_y = 0, end_y = 0;
    float start_z = 0, end_z = 0;
    pPointCloud output_cloud(new PointCloud);

    if(start_point.x < end_point.x){start_x = start_point.x; end_x = end_point.x;}
    if(start_point.y < end_point.y){start_y = start_point.y; end_y = end_point.y;}
    if(start_point.z < end_point.z){start_z = start_point.z; end_z = end_point.z;}
    for (pcl::PointCloud<pcl::PointXYZ>::iterator it = cloud_points->begin (); it != cloud_points->end (); ++it)
    {
        if(it->x >= start_x && it->x <= end_x)
        {
            if(it->x >= start_x && it->x <= end_x)
            {
                if(it->x >= start_x && it->x <= end_x)
                {
                    output_cloud->points.push_back(*it);
                }
            }
        }
    }

    return output_cloud;
}

/**
 * @brief 点云旋转平移
 * @param 输入点云，平移坐标，旋转坐标
 * @return 无
 * @details 平移单位米，正方向右前下（人与摄像头朝向一致），旋转单位度，正方向符合右手定则（握住旋转轴，大拇指指向旋转轴正方向）
 *          不必考虑先旋转还是先平移的问题，转换函数中已经考虑进去了
*/
void PclProcess::cloud_translation(pPointCloud cloud_points, float trans_x, float trans_y, float trans_z, 
                                                            float rotate_x, float rotate_y, float rotate_z)
{
    Eigen::Affine3f transform = Eigen::Affine3f::Identity();
    transform.translation() << trans_x, trans_y, trans_z; // 平移
    transform.rotate (Eigen::AngleAxisf (M_PI * rotate_x / 180.f, Eigen::Vector3f::UnitX())); // 旋转
    transform.rotate (Eigen::AngleAxisf (M_PI * rotate_y / 180.f, Eigen::Vector3f::UnitY()));
    transform.rotate (Eigen::AngleAxisf (M_PI * rotate_z / 180.f, Eigen::Vector3f::UnitZ()));
    pcl::transformPointCloud (*cloud_points, *cloud_points, transform); // 应用输出
}
// 单个点平移旋转
PointType PclProcess::cloud_translation(PointType cloud_point, float trans_x, float trans_y, float trans_z, 
                                                            float rotate_x, float rotate_y, float rotate_z)
{
    Eigen::Affine3f transform = Eigen::Affine3f::Identity();
    transform.translation() << trans_x, trans_y, trans_z; // 平移
    transform.rotate (Eigen::AngleAxisf (M_PI * rotate_x / 180.f, Eigen::Vector3f::UnitX())); // 旋转
    transform.rotate (Eigen::AngleAxisf (M_PI * rotate_y / 180.f, Eigen::Vector3f::UnitY()));
    transform.rotate (Eigen::AngleAxisf (M_PI * rotate_z / 180.f, Eigen::Vector3f::UnitZ()));
    return pcl::transformPoint (cloud_point, transform); // 应用输出
}

/**
 * @brief 根据车体坐标、位姿、转台角度将点相对于摄像头的坐标转换到世界坐标
 * @param 车体xy坐标，车体偏航角，转台偏航角，相对于摄像头的点坐标
 * @return 世界坐标系下的点坐标
 * @details 
*/
PointType PclProcess::Ball_Relative_To_World_Coordinate(float car_x, float car_y, float car_yaw, float table_yaw, PointType input_point)
{

    float l1 = 0.110;          // 车中心到转盘中心的y方向水平距离
    float l4 = 0.50755;       // 摄像头竖直高度 原高度为 0.46755
    float l5 = 0.35486;       // 摄像头距离转盘中心的水平垂直距离
    // float car_x = -0.79;        // 车相对于原点的x
    // float car_y = 1.0;        // 车相对于原点的y
    float theta1 = table_yaw / 180.0 * PCL_PAI; // 转盘偏航
    float theta4 = 29.f / 180.f * PCL_PAI;     // 摄像头上倾角(定值)
    float theta5 = car_yaw / 180.0 * PCL_PAI;   // 车的偏航角

    Eigen::Matrix<double,4,4> Tm0_1, Rm0_1, Tm1_2, Tm2_3, Rm2_3, Tm3_4, Tm4_5, Rm4_5;
    Eigen::Vector4d pm3_3, pm3_0, pm5_0;

    Tm0_1 <<    1, 0, 0, 0,
                0, 1, 0, l1,
                0, 0, 1, 0,
                0, 0, 0, 1;
                
    Rm0_1 <<    cos(theta1), -sin(theta1), 0, 0,
                sin(theta1), cos(theta1), 0, 0,
                0, 0, 1, 0,
                0, 0, 0, 1;

    Tm1_2 <<    1, 0, 0 ,0,
                0, 1, 0, 0,
                0, 0, 1, l4,
                0, 0, 0, 1;
    
    Tm2_3 <<    1, 0, 0, 0,
                0, 1, 0, -l5,
                0, 0, 1, 0,
                0, 0, 0, 1;
                    
    Rm2_3 <<    1, 0, 0, 0,
                0, cos(theta4), -sin(theta4), 0,
                0, sin(theta4), cos(theta4), 0,
                0, 0, 0, 1;
                
    pm3_3 << -input_point.y, input_point.z, -input_point.x, 1;   // 球相对于摄像头的坐标 

    pm3_0 = Tm0_1 * Rm0_1 * Tm1_2 * Tm2_3 * Rm2_3 * pm3_3; // 球相对于车原点的坐标

    Tm3_4 <<    1, 0, 0, car_x,
                0, 1, 0, 0,
                0, 0, 1, 0,
                0, 0, 0, 1;
                    
    Tm4_5 <<    1, 0, 0, 0,
                0, 1, 0, car_y,
                0, 0, 1, 0,
                0, 0, 0, 1;
                    
    Rm4_5 <<    cos(theta5), -sin(theta5), 0, 0,
                sin(theta5), cos(theta5), 0, 0,
                0, 0, 1, 0,
                0, 0, 0, 1;
                    
    pm5_0 = Tm3_4 * Tm4_5 * Rm4_5 * pm3_0;  // 球相对于原点的坐标

    // cout << "pm5_0:" << pm5_0[0] << endl;
    PointType result(pm5_0[0], -pm5_0[2], pm5_0[1]);
    return result;
}

/**
 * @brief 点云清空
 * @param 
 * @return 
 * @details 
*/
void PclProcess::cloud_point_clear(void)
{
    roi_cloud->points.clear();
    center_cloud->points.clear();
    cloud_sphere->points.clear();
}

PclProcess::PclProcess(const std::string &name):
roi_cloud(new PointCloud),
center_cloud(new PointCloud),
cloud_sphere(new PointCloud),
center_point(0,0,0)
{
    Pcl_show_init();
    viewer.setWindowName(name);
    // cloud_allpath = getpath(CLOUDPATH);
}
PclProcess::~PclProcess()
{
}